#include "board_com.h"
#include "can_common.h"
#include "controller.h"
#include "dji.h"
#include "drivers/dev_can.h"
#include "motor_def.h"
#include "offline.h"
#include "omni.h"
#include "rtdef.h"
#include "rtthread.h"
#include "rttypes.h"
#include "robotdef.h"
#include "robot_cmd.h"
#include "user_lib.h"

#define LOG_TAG              "chassis"
#define LOG_LVL              LOG_LVL_DBG
#include <ulog.h>


static Chassis_Ctrl_Cmd_s chassis_cmd_recv;         // 底盘接收到的控制命令 
static Chassis_Upload_Data_s Chassis_Upload_Data;
static DJIMotor_t *motor_lf, *motor_rf, *motor_lb, *motor_rb; // left right forward back
/* 私有函数计算的中介变量,设为静态避免参数传递的开销 */
static float chassis_vx, chassis_vy;     // 将云台系的速度投影到底盘
static float vt_lf, vt_rf, vt_lb, vt_rb; // 底盘速度解算后的临时输出,待进行限幅
extern board_com_s board_com;
static PIDInstance chassis_follow_pid;

#define HALF_WHEEL_BASE (WHEEL_BASE / 2.0f)     // 半轴距
#define HALF_TRACK_WIDTH (TRACK_WIDTH / 2.0f)   // 半轮距
#define PERIMETER_WHEEL (RADIUS_WHEEL * 2 * PI) // 轮子周长

#define LF_CENTER ((HALF_TRACK_WIDTH + CENTER_GIMBAL_OFFSET_X + HALF_WHEEL_BASE - CENTER_GIMBAL_OFFSET_Y) * DEGREE_2_RAD)
#define RF_CENTER ((HALF_TRACK_WIDTH - CENTER_GIMBAL_OFFSET_X + HALF_WHEEL_BASE - CENTER_GIMBAL_OFFSET_Y) * DEGREE_2_RAD)
#define LB_CENTER ((HALF_TRACK_WIDTH + CENTER_GIMBAL_OFFSET_X + HALF_WHEEL_BASE + CENTER_GIMBAL_OFFSET_Y) * DEGREE_2_RAD)
#define RB_CENTER ((HALF_TRACK_WIDTH - CENTER_GIMBAL_OFFSET_X + HALF_WHEEL_BASE + CENTER_GIMBAL_OFFSET_Y) * DEGREE_2_RAD)
/**
 * @brief 计算每个轮毂电机的输出,正运动学解算
 *        用宏进行预替换减小开销,运动解算具体过程参考教程
 */
static void OmniCalculate()
{
    vt_lf = chassis_vx - chassis_vy - chassis_cmd_recv.wz * LF_CENTER;
    vt_rf = -chassis_vx - chassis_vy - chassis_cmd_recv.wz * RF_CENTER;
    vt_lb = -chassis_vx + chassis_vy - chassis_cmd_recv.wz * LB_CENTER;
    vt_rb = chassis_vx + chassis_vy - chassis_cmd_recv.wz * RB_CENTER;

}

#define chassis_follow_Kp 10.0f  // 比例增益
#define chassis_follow_Ki 100.0f  // 积分增益
#define chassis_follow_Kd 1.0f // 微分增益
// PID控制器状态变量
static float integral = 0.0f;
static float prev_error = 0.0f;
static float PIDControl(float error, float dt)
{
    // 计算积分项
    integral += error * dt;
    // 计算微分项
    float derivative = (error - prev_error) / dt;
    // 计算PID输出
    float output = chassis_follow_Kp * error + chassis_follow_Ki * integral + chassis_follow_Kd * derivative;
    // 更新前一次误差
    prev_error = error;
    return output;
}

void ChassisInit()
{
    board_com.board_tx.can_handle=&can1_bus;
    board_com.board_tx.txmsg.id=BOARD_CHASSIS_ID;
    board_com.board_tx.txmsg.ide =RT_CAN_STDID;
    board_com.board_tx.txmsg.rtr =RT_CAN_DTR;
    board_com.board_tx.txmsg.len =8;
    board_com.send_data_len = sizeof(Chassis_Upload_Data);
    board_com.recv_data_len = sizeof(chassis_cmd_recv);
    board_com.recv_buf_len = board_com.recv_data_len + CAN_COMM_OFFSET_BYTES; // head + datalen + crc8 + tail
    board_com.send_buf_len = board_com.send_data_len + CAN_COMM_OFFSET_BYTES;
    board_com.raw_sendbuf[1] = board_com.send_data_len; // datalen
    board_com.raw_sendbuf[board_com.send_data_len + CAN_COMM_OFFSET_BYTES - 1] = CAN_COMM_TAIL;

    PID_Init_Config_s config = {.MaxOut = 10000,
                                .IntegralLimit = 200,
                                .DeadBand = 0.1,
                                .Kp = 50,
                                .Ki = 200,
                                .Kd = 1,
                                .Improve = 0x01}; // enable integratiaon limit
    PIDInit(&chassis_follow_pid, &config);

    
    // 四个轮子的参数一样,改tx_id和反转标志位即可
    Motor_Init_Config_s chassis_motor_config = {
        .offline_manage_motor ={
            .enable = OFFLINE_ENABLE,
            .online_state = STATE_OFFLINE,
            .error_level =OFFLINE_ERROR_LEVEL,
            .last_time =rt_tick_get(),
            .offline_time =1000,
        },
        .can_init_config.can_handle = &can2_bus,
        .controller_param_init_config = {
            .speed_PID = {
                .Kp = 4.5, // 4.5
                .Ki = 0,  // 0
                .Kd = 0,  // 0
                .IntegralLimit = 3000,
                .Improve = PID_Trapezoid_Intergral | PID_Integral_Limit | PID_Derivative_On_Measurement,
                .MaxOut = 12000,
            },
            .current_PID = {
                .Kp = 0.4, // 0.4
                .Ki = 0,   // 0
                .Kd = 0,
                .IntegralLimit = 3000,
                .Improve = PID_Trapezoid_Intergral | PID_Integral_Limit | PID_Derivative_On_Measurement,
                .MaxOut = 15000,
            },
        },
        .controller_setting_init_config = {
            .angle_feedback_source = MOTOR_FEED,
            .speed_feedback_source = MOTOR_FEED,
            .outer_loop_type = SPEED_LOOP,
            .close_loop_type = SPEED_LOOP | CURRENT_LOOP,
        },
        .motor_type = M3508,
    };

    chassis_motor_config.can_init_config.tx_id = 1;
    chassis_motor_config.controller_setting_init_config.motor_reverse_flag = MOTOR_DIRECTION_NORMAL;
    chassis_motor_config.offline_manage_motor.event = OFFLINE_CHASSIS_MOTOR1;
    chassis_motor_config.offline_manage_motor.beep_times = 1;
    motor_lf = DJIMotorInit(&chassis_motor_config);

    chassis_motor_config.can_init_config.tx_id = 2;
    chassis_motor_config.controller_setting_init_config.motor_reverse_flag = MOTOR_DIRECTION_NORMAL;
    chassis_motor_config.offline_manage_motor.event = OFFLINE_CHASSIS_MOTOR2;
    chassis_motor_config.offline_manage_motor.beep_times = 2;
    motor_rf = DJIMotorInit(&chassis_motor_config);

    chassis_motor_config.can_init_config.tx_id = 3;
    chassis_motor_config.controller_setting_init_config.motor_reverse_flag = MOTOR_DIRECTION_NORMAL;
    chassis_motor_config.offline_manage_motor.event = OFFLINE_CHASSIS_MOTOR3;
    chassis_motor_config.offline_manage_motor.beep_times = 3;
    motor_rb = DJIMotorInit(&chassis_motor_config);

    chassis_motor_config.can_init_config.tx_id = 4;
    chassis_motor_config.controller_setting_init_config.motor_reverse_flag = MOTOR_DIRECTION_NORMAL;
    chassis_motor_config.offline_manage_motor.event = OFFLINE_CHASSIS_MOTOR4;
    chassis_motor_config.offline_manage_motor.beep_times = 4;
    motor_lb = DJIMotorInit(&chassis_motor_config);

}

static void chassis_thread_entry(void *parameter)
{
    static float prev_wz;
    rt_thread_mdelay(2000);
    ChassisInit();
    while (1) {
        chassis_cmd_recv = *(Chassis_Ctrl_Cmd_s *)CANCommGet();
        //LOG_D("chassis mode: %d,chassis vx:%f,vy:%f,wz:%f\n",chassis_cmd_recv.chassis_mode,chassis_cmd_recv.vx,chassis_cmd_recv.vy,chassis_cmd_recv.wz);
        if (chassis_cmd_recv.chassis_mode == CHASSIS_ZERO_FORCE)
        { // 如果出现重要模块离线或遥控器设置为急停,让电机停止
            DJIMotorSetRef(motor_lf,0);
            DJIMotorSetRef(motor_lb,0);
            DJIMotorSetRef(motor_rf,0);
            DJIMotorSetRef(motor_rb,0);
            DJIMotorStop(motor_lf);
            DJIMotorStop(motor_rf);
            DJIMotorStop(motor_lb);
            DJIMotorStop(motor_rb);
        }
        else
        { // 正常工作
            DJIMotorEnable(motor_lf);
            DJIMotorEnable(motor_rf);
            DJIMotorEnable(motor_lb);
            DJIMotorEnable(motor_rb);
        }

        // 根据控制模式设定旋转速度
        switch (chassis_cmd_recv.chassis_mode)
        {
        case CHASSIS_NOFOLLOW_GIMBAL_YAW: // 自旋反转,同时保持全向机动;当前wz维持定值,后续增加不规则的变速策略
            chassis_cmd_recv.wz = 0;
            chassis_cmd_recv.offset_angle =0.0f;
            break;
        case CHASSIS_FOLLOW_GIMBAL_YAW: // 跟随云台,不单独设置pid,以误差角度平方为速度输出
            PIDCalculate(&chassis_follow_pid,chassis_cmd_recv.offset_angle,0);
            chassis_cmd_recv.wz = -chassis_follow_pid.Output;
            break;
        case CHASSIS_ROTATE: // 自旋,同时保持全向机动;当前wz维持定值,后续增加不规则的变速策略
            //chassis_cmd_recv.wz = 8000;
            break;
        default:
            break;
        }

        // 根据云台和底盘的角度offset将控制量映射到底盘坐标系上
        // 底盘逆时针旋转为角度正方向;云台命令的方向以云台指向的方向为x,采用右手系(x指向正北时y在正东)
        static float sin_theta, cos_theta;
        cos_theta = arm_cos_f32(chassis_cmd_recv.offset_angle * DEGREE_2_RAD);
        sin_theta = arm_sin_f32(chassis_cmd_recv.offset_angle * DEGREE_2_RAD);
        chassis_vx = chassis_cmd_recv.vx * cos_theta - chassis_cmd_recv.vy * sin_theta;
        chassis_vy = chassis_cmd_recv.vx * sin_theta + chassis_cmd_recv.vy * cos_theta;

        OmniCalculate();
        DJIMotorSetRef(motor_lf, vt_lf);
        DJIMotorSetRef(motor_rf, vt_rf);
        DJIMotorSetRef(motor_lb, vt_lb);
        DJIMotorSetRef(motor_rb, vt_rb);

        Chassis_Upload_Data.real_vx = 100;
        Chassis_Upload_Data.real_vy = 1;
        board_send((rt_uint8_t *)&Chassis_Upload_Data);

        rt_thread_mdelay(5);
    }
}



int chassis_thread_entry_init(void)
{
    rt_err_t ret = RT_EOK;
    rt_thread_t thread = rt_thread_create("chassis", chassis_thread_entry, RT_NULL, 2048, 31, 10);
    /* 创建成功则启动线程 */
    if (thread != RT_NULL)
    {
        rt_thread_startup(thread);
    }
    else
    {
        ret = RT_ERROR;
    }

    LOG_D("chassis thread init success!\n");

    return ret;
}

#ifdef BOARD_CHASSIS
INIT_APP_EXPORT(chassis_thread_entry_init);
#endif // BOARD_CHASSIS
